#ifndef _ARTICULATED_BODY_H
#define _ARTICULATED_BODY_H

#include "stdafx.h"
#include "DecoSolidObject.h"
#include "RenderInterface.h"
#include "RenderType.h"
#include "Model.h"
#include "ModelFileInfo.h"
#include "SkelMesh.h"
#include "SkeletonScriptReader.h"
#include <string>
#include "ForwardDynamics.h"
#include "SearcherConfig.h"

enum ArticulatedSimType
{
	AST_Cartesian,
	AST_General
};

class IndexJacobianList
{
public:
	vector<Index3> mIndices;
	vector<Matd> mJacobian;
	int Size() const;
	void AddElement(const Index3& index, const Matd& jacobian);
	bool FindElement(const Index3& index, Matd& jacobian);
	void Clear();
};

//#define WATER_OFF

class DecoArticulatedObject :
	public DecoSolidObject
{
public:
	DecoArticulatedObject(void);
	virtual ~DecoArticulatedObject(void);

public:
	static DecoArticulatedObject* CreateFromMesh(const std::string & path);
	
	void Reset();
	friend DecoArchive& operator<< (DecoArchive& Ar, const DecoArticulatedObject& obj);
	friend DecoArchive& operator>> (DecoArchive& Ar, DecoArticulatedObject& obj);
	void ReadFromSkeleton(const std::string& path);
	virtual DecoRenderData* GetRenderData(int ithBody);
	virtual DecoRenderData* GetRenderData();
	void UpdateTransformation();
	void Update(DOUBLE deltaTime);
	vector3 SampleVelocity(const vector3& pos);
	Model* GetModel() const;
	int GetNumDofs() const;
	int GetNumBodies() const;
	void UpdateBeforeCoupling(DOUBLE deltaTime, BOOL bSimplifiedFluid = FALSE);
	void UpdateToTarget();
	void SetSimulationType(ArticulatedSimType type);
	bool GetJacobianAtUFace(int i, int j, int k, Matd& jacobian);
	bool GetJacobianAtVFace(int i, int j, int k, Matd& jacobian);
	bool GetJacobianAtWFace(int i, int j, int k, Matd& jacobian);
	double GetMass();
	void SetQDot(const std::vector<double>& qdot, bool bSetFirstSixDofs = false);
	void GetQDot(std::vector<double>& qdot);
	void GetQ(std::vector<double>& q);
	void GetDesiredQandQDot(double time, std::vector<double>& q, std::vector<double>& qdot);
 	typedef vector<Index3> IndiceListT;
	typedef vector<IndiceListT> IndiceListForBodiesT;
	IndiceListForBodiesT mGridIndicesForBodies;
	virtual FluidSparseVector GetPressureContributionToUFaceAcc(int i, int j, int k, int dim);
	virtual FluidSparseVector GetPressureContributionToVFaceAcc(int i, int j, int k, int dim);
	virtual FluidSparseVector GetPressureContributionToWFaceAcc(int i, int j, int k, int dim);
	virtual void PrepareImplicitTwowayCouplingMatrix(int dim);
	bool SetMotionScriptAmplitude(const std::string& dofName, double value);
	bool SetMotionScriptPeriod(const std::string& dofName, double value);
	bool SetMotionScriptPhase(const std::string& dofName, double value);
	bool SetMotionScriptConstant(const std::string& dofName, double value);
	bool SetMotionScriptSpline(const std::string& dofName, double period, const std::vector<double>& ctrlPts);
	bool SetMotionScriptSineWarpControlPts(const std::string& name, const std::vector<double>& ctrPts);
	vector3 GetCumulativeEnergyConsumption();
	void CumulateEnergyConsumption(double energy, double torqueAbs, double torqueSqr);
	void CumulateDeviationFromCircle(const vector3& center, double radius, double omega, double currentTime);
	double GetCumulativeDeviation();
	void CumulateCollisionVolume(double vol);
	double GetCumulativeCollisionVolume();

	SkeletonScriptReader* GetMotionScript() const;
	Matd calculateJacobian(BodyLink* b, const vector3& localPos);
	Vecd convertForceFromCartesianToGeneral(BodyLink* b, const vector3& posLocal, const vector3& force);
	void SetMotionScript(SkeletonScriptReader* motionScript);
	void ApplyMotion(const SearcherInfo& info, double* params, int numArray);
	TrackMode mMode;

	double mRotTorque;

private:
	void updateInGeneralCoord(DOUBLE deltaTime);
	void updateInCartesianCoord(DOUBLE deltaTime);
	void updateRigidBody(const vector3& force, const vector3& torque, DOUBLE deltaTime);

	Model* mModel;
	ModelFileInfo* mModelInfo;
	DecoSkelMesh* mMesh;
	SkeletonScriptReader* mMotionScript;
	double mCurrentTime;
	matrix44 mLocalToWorld;
	ForwardDynamics* mSimulator;
	ArticulatedSimType mSimulateType;
	FluidSparseVector* mGeneralizedForce;
	IndexJacobianList mUJacobianList;
	IndexJacobianList mVJacobianList;
	IndexJacobianList mWJacobianList;
	double mCumulativeEnergy;
	double mCumulativeTorqueAbs;
	double mCumulativeTorqueSqr;
	double mCumulativeDeviation;
	double mCumulativeCollisionVol;

public:
	double mCumulativeIndexedEnergy[10];
	vector3 mPosFirstCycle;
	vector3 mOriFirstCycle;
	vector3 mDirFirstCycle;
};
#endif
